/**
 * @file    soc_calc.c
 * @brief   SOC计算
 * @author  Liu wei
 * @version 1.0.1
 * @date    2023-04-19
 * 
 * @copyright Copyright (c) 2023 JBD-Energy Storage Technology Co. LTD
 * 
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Version   <th>Author   <th>Description
 * <tr><td>2023-04-19 <td>1.0.1     <td>Liuwei   <td>首次创建
 * </table>
 */

/* includes ------------------------------------------------------------------*/
#include "soc_calc.h"
#include "sample.h"
#include "param_mng.h"
#include "cfg_prj.h"
#include "var_mng.h"
#include "diagnosis.h"
#include "drv_gpt.h"
#include "drv_gpio.h"
#include "soh_calc.h"
#include "base_math.h"
/* macro/define --------------------------------------------------------------*/
#define DISP_100_MIN          (9920)
#define CHG_LIMIT_SOC         (9910)  /* 98% */
#define DSG_LIMIT_SOC         (120)   /* 1.2% */

#define DSG_LIMIT_UP_VOLT     (3150)
#define DSG_LIMIT_UP_SOC      (1550)   /* 15% */

#define DSG_LIMIT_MID_VOLT    (3150)
#define DSG_LIMIT_MID_SOC     (1500)   /* 15% */

#define DSG_UP_SPEED          (2.0)    /* 向下追赶系数 */
#define DSG_DOWN_SPEED        (0.5)    /* 向上追赶系数 */

#define FULL_CHG_DLY          (30000)  /* 满充校准延时 */

#define DSG_SPEED_CURR        (0.5f)  
#define DSG_SPEED_COEFF1      (1.03f) /*!< 放电加速系数 */
#define DSG_SPEED_CURR2       (1.0f)  
#define DSG_SPEED_COEFF2      (1.05f) /*!< 放电加速系数 */
#define DSG_SPEED_CURR3       (0.5f)  
#define DSG_SPEED_COEFF3      (1.08f) /*!< 放电加速系数 */

#define CHG_SPEED_COEFF       (1.03f) /*!< 充电加速系数 */

#define SOC_CURR_STEP         (7)
#define SOC_VOLT_STEP         (31)
#define NO_FULL_CHG_TIME      (2592000000)  /* 30天 */
/* typedef -------------------------------------------------------------------*/
typedef struct 
{
    uint8_t jump_flag ;              /* !< SOC跳变状态*/
    double rcap_mah_disp;            /* !<显示剩余容量 */
    double rcap_mah_now ;            /* !<跳变剩余容量 */
    double now_real_soc;    
    double acc_chg_mah ;             /* !<累计充电AH */
    double acc_dsg_mah ;             /* !<累计放电AH */
    double acc_con_chg_mah ;         /* !<持续充电累计AH数 */
    double acc_con_dsg_mah ;         /* !<持续放电累计AH数 */
    double acc_iddq_mah ;            /* !<静态累计功耗 */
    double soh_acc_mah ;             /* !<soh计算累计 */
}soc_data_type;

enum
{
    FCC_IDLE  ,
    FCC_START ,
    FCC_CMP   , 
    FCC_CAP_ERR ,  
};

enum
{
    SOC_TRACE_IDLE  ,  /* 未追踪 */
    SOC_TRACE_RUN   ,  /* 正在追踪 */
    SOC_TRACE_UP_RUN ,	/* 正在缓慢下降 */
    SOC_TRACE_CMP   ,  /* 追踪完成*/
};

typedef struct 
{
	uint16_t volt;
	uint16_t soc;
}soc_volt_type;

/* local functions -----------------------------------------------------------*/
static float soc_get_dsg_speed(float curr);
static uint16_t soc_find_actual(float bus_curr , uint16_t volt);
static float soc_get_chg_speed(uint16_t soc_diff);
/* static variables ----------------------------------------------------------*/
static soc_data_type soc_calc_data = {0};
static uint8_t fcc_state = FCC_IDLE;
static uint8_t soc_trace_state = 0;
static uint32_t full_chg_dly = 0;
static uint32_t no_full_chg_cnt = 0; 
/* 电流对应表 */
static const float soc_curr_table[SOC_CURR_STEP] = 
{
	0.1 , 0.2 , 0.4 , 0.5 , 0.6 , 0.8 , 1.0 
};
static uint16_t soc_volt_calc_buf[SOC_VOLT_STEP] = {0};

/* 电压对应的SOC表 */
static const uint16_t soc_volt_table[SOC_CURR_STEP][SOC_VOLT_STEP] = 
{
	{ 3198,3197,3194,3191,3190,3187,3184,3180,3177,3174,
	  3170,3166,3162,3157,3151,3146,3140,3136,3131,3128,
	  3125,3121,3116,3110,3098,3078,3041,2988,2910,2905,2000 } , /* 0.1C*/
	
	{ 3198,3197,3194,3191,3190,3187,3184,3180,3177,3174,
	  3170,3166,3162,3157,3151,3146,3140,3136,3131,3128,
	  3125,3121,3116,3110,3098,3078,3041,2988,2910,2905,2000 } , /* 0.2C*/	
	
	{ 3198,3197,3194,3191,3190,3187,3184,3180,3177,3174,
	  3170,3166,3162,3157,3151,3146,3140,3136,3131,3128,
	  3125,3121,3116,3110,3098,3078,3041,2988,2910,2905,2000 } ,  /* 0.4C*/	
	
	{ 3182,3180,3177,3174,3173,3170,3167,3164,3160,3157,
	  3153,3149,3145,3141,3135,3131,3125,3121,3116,3112,
	  3108,3103,3097,3090,3077,3056,3023,2970,2910,2905,2000 } ,  /* 0.5C*/
	
	{ 3165,3162,3160,3157,3155,3152,3150,3147,3143,3140,
	  3136,3132,3128,3124,3119,3115,3110,3105,3101,3096,
	  3091,3085,3078,3069,3055,3034,3005,2952,2910,2905,2000 } ,  /* 0.6C*/
	
	{ 3139,3137,3134,3132,3129,3127,3125,3122,3118,3115,
	  3111,3108,3104,3100,3096,3091,3088,3083,3078,3073,
	  3067,3060,3051,3040,3022,3006,2974,2927,2910,2905,2000 } ,  /* 0.8C*/
	
	{ 3100,3099,3096,3094,3091,3090,3087,3084,3081,3078,
	  3075,3072,3068,3064,3061,3056,3051,3047,3041,3035,
	  3027,3015,3015,3001,2982,2968,2945,2935,2930,2905,2000 } ,  /* 1.0C*/	
};

/* 充电曲线表 */
const soc_volt_type soc_volt_chg_map[] = 
{
	{3460 , 94  } ,
	{3470 , 95  } ,
	{3480 , 96  } ,
	{3490 , 97  } ,
	{3520 , 98  } ,
	{3530 , 99  } ,	
	{3650 , 100  } ,
};

#define SOC_VOLT_SIZE      (sizeof(soc_volt_chg_map) / sizeof(soc_volt_type))
	
/* global variables ----------------------------------------------------------*/

/** ------------------------------------------------------------------------- *
  *                             Global function
 ** ------------------------------------------------------------------------- */
void soc_calc_init(void)
{
    uint16_t now_soc = 0;
    uint16_t full_cap = 0;
    uint32_t var_data = 0;
	
    var_set_data(VAR_ID_BUS_CURR , VAR_CURR_OFFSET / VAR_CURR_FACTOR);
    now_soc = param_get_data(PAR_ID_RSOC);
    var_set_data(VAR_ID_DISP_SOC , now_soc);
//    var_set_data(VAR_ID_RSOC , now_soc);
	var_set_data(VAR_ID_DISP_RCAP , now_soc * 0.0001 * param_get_data(PAR_ID_FULL_CAP));
	var_set_data(VAR_ID_REAL_CAP , param_get_data(PAR_ID_REAL_CAP));

    full_cap = param_get_data(PAR_ID_FULL_CAP);
    soc_calc_data.rcap_mah_disp = now_soc * full_cap * 0.001;
    soc_calc_data.rcap_mah_now = soc_calc_data.rcap_mah_disp;  //显示等于真实
    
    var_data = (param_get_data(PAR_ID_ACC_CHG_AH_H) << 16) + param_get_data(PAR_ID_ACC_CHG_AH_L);
    soc_calc_data.acc_chg_mah = var_data * 100; 
	var_set_data(VAR_ID_ACC_CHG_AH_H , param_get_data(PAR_ID_ACC_CHG_AH_H));
	var_set_data(VAR_ID_ACC_CHG_AH_L , param_get_data(PAR_ID_ACC_CHG_AH_L));
	
    var_data = (param_get_data(PAR_ID_ACC_DSG_AH_H) << 16) + param_get_data(PAR_ID_ACC_DSG_AH_L);
	var_set_data(VAR_ID_ACC_DSG_AH_H , param_get_data(PAR_ID_ACC_DSG_AH_H));
	var_set_data(VAR_ID_ACC_DSG_AH_L , param_get_data(PAR_ID_ACC_DSG_AH_L));
    soc_calc_data.acc_dsg_mah = var_data * 100;   
	
    soc_calc_data.acc_con_chg_mah = 0;        
    soc_calc_data.acc_con_dsg_mah = 0;        
    soc_calc_data.acc_iddq_mah = 0; 

	drv_gpt_init(DRV_GPT2 , 50 , &soc_calc_ampere);
}

/**
 * @brief  安时积分计算SOC
 * @param  time : 计算周期
 */   
void soc_calc_ampere(uint32_t time)
{
	double bus_curr;
	double calc_incr = 0;
    double actual_incr =0 ;
	int32_t curr_val = 0;  
	uint16_t calc_soc = 0;
	float limit_mah = 0;	 
	uint16_t soc_cfg = 0;
	uint8_t i = 0;
	uint16_t max_volt = 0;
	uint16_t actual_soc = 0;
	uint16_t disp_soc = 0;
	float chg_speed = 0;
	float mid_spedd = 0;
	
	calc_soc = time;
    bus_curr = var_get_data(VAR_ID_BUS_CURR) * VAR_CURR_FACTOR - VAR_CURR_OFFSET;
    curr_val = (int32_t)(bus_curr * 1000);
	/* 放电SOC计算 */
    if(curr_val < 0)  
    {
        /* 清除充电累计状态 */
        soc_calc_data.acc_con_chg_mah = 0;
		soc_calc_data.acc_iddq_mah = 0;
        /* 计算SOC实际增量 */
        actual_incr = (double)(-bus_curr) * time / 3600.0;
		soc_calc_data.acc_dsg_mah += actual_incr;
        soc_calc_data.soh_acc_mah += actual_incr;
        soc_calc_data.acc_con_dsg_mah += actual_incr;
		limit_mah = param_get_data(PAR_ID_FULL_CAP) * 0.001 *  DSG_LIMIT_SOC;
        calc_incr = actual_incr;
       
		// 如果配置了 SOC校准算法，则判断
		param_get_4bits_val(PAR_ID_SOC_CFG ,FUNC_SOC_TRACE , &soc_cfg); 
		if(PARAM_SOC_CFG_EN != soc_cfg)
		{
			if((var_get_data(VAR_ID_MIN_CELL_VOLT) > DSG_LIMIT_UP_VOLT) //15%3080
					&&( var_get_data(VAR_ID_DISP_SOC)  < DSG_LIMIT_UP_SOC))
			{
				calc_incr = 0;//0.6;
			}
			else
			{
				if(param_get_data(PAR_ID_STD_CAP) < 30000)	
				{	
					/* soc 追赶 */
					if(SOC_TRACE_RUN == soc_trace_state)
					{          
						disp_soc = var_get_data(VAR_ID_DISP_SOC) * 0.01;
						actual_soc = var_get_data(VAR_ID_RSOC); 
						// 如果SOC相差1% ，则追踪否则不追踪。
						if(disp_soc != actual_soc)
						{
							if(disp_soc > actual_soc)
							{
								mid_spedd = (disp_soc - actual_soc) * 0.05 + 1.5;
								calc_incr = calc_incr * mid_spedd;	 // 向上追赶 
							}
							else if(actual_soc >= disp_soc)
							{
								mid_spedd = (actual_soc - disp_soc ) * 0.05 + 1.5;
								calc_incr = calc_incr / mid_spedd;	// 向下追赶  
							}
							else
							{
								calc_incr = actual_incr * soc_get_dsg_speed(bus_curr);//正常追踪
							}
						}
						else
						{
							calc_incr = actual_incr * soc_get_dsg_speed(bus_curr);//正常追踪
						}
					}
					else
					{
						calc_incr = actual_incr * soc_get_dsg_speed(bus_curr);//正常追踪
					}
				}
				else
				{
					calc_incr = actual_incr * soc_get_dsg_speed(bus_curr);//正常追踪
				}			
			}
        }
		else
		{
			calc_incr = actual_incr * soc_get_dsg_speed(bus_curr);	
		}
		
		/* 系统未触发单体欠压或总压欠压告警及保护限制SOC */
		if(((diag_get_alarm(ALM_ID_BUV) && diag_get_prp(PRP_ID_CUV) && diag_get_prp(PRP_ID_BUV) && diag_get_alarm(ALM_ID_BUV)) == 0)
			&&((soc_calc_data.rcap_mah_disp - calc_incr) < limit_mah))
		{
			;
		}
		else
		{
			if(soc_calc_data.rcap_mah_disp > calc_incr) 
				soc_calc_data.rcap_mah_disp -= calc_incr; 
       		else
          		soc_calc_data.rcap_mah_disp = 0;
		}
		
		calc_soc = 10000 * soc_calc_data.rcap_mah_disp / (param_get_data(PAR_ID_FULL_CAP) * 10);
		// SOC 大于99.20 显示 100%,防止上电精度丢失导致显示异常
		if(calc_soc > DISP_100_MIN)
			calc_soc = 10000;
		
		var_set_data(VAR_ID_DISP_SOC , calc_soc);
        var_set_data(VAR_ID_DISP_RCAP , soc_calc_data.rcap_mah_disp * 0.1);
    }
    else if(curr_val > 0)  /** 充电逻辑*/
    {
        /* 清除放电累计状态 */
        soc_calc_data.acc_con_dsg_mah = 0;
		soc_calc_data.acc_iddq_mah = 0;
        /* 计算得到 mAH */
        actual_incr = (double)(bus_curr) * time / 3600.0;
        calc_incr = actual_incr;
        /* 计算累计充电AH数 */
        soc_calc_data.acc_con_chg_mah += actual_incr;
        soc_calc_data.acc_chg_mah += actual_incr;
		limit_mah = param_get_data(PAR_ID_FULL_CAP) * 0.001 * CHG_LIMIT_SOC;	
		/* 如果单体电压未达到限制SOC，则系统限制SOC在99% */
		if(((diag_get_alarm(ALM_ID_COV) && diag_get_prp(PRP_ID_COV) && diag_get_prp(PRP_ID_BOV) && diag_get_alarm(ALM_ID_BOV)) == 0)
			&&(soc_calc_data.rcap_mah_disp >= limit_mah))
		{
			;
		}
		else 
		{
			param_get_4bits_val(PAR_ID_SOC_CFG ,FUNC_SOC_TRACE , &soc_cfg); 
			if(PARAM_SOC_CFG_EN != soc_cfg)
			{
				max_volt = var_get_data(VAR_ID_MAX_CELL_VOLT);
				disp_soc = var_get_data(VAR_ID_DISP_SOC) * 0.01;
				if((max_volt < soc_volt_chg_map[0].volt)&& (disp_soc < soc_volt_chg_map[0].soc))//与SOC
				{
					calc_incr = calc_incr* CHG_SPEED_COEFF;
				}
				else
				{
					// 内部追踪 
					for(i = 0; i < SOC_VOLT_SIZE; ++ i)
					{
						if(max_volt < soc_volt_chg_map[i].volt)
						{
							actual_soc = soc_volt_chg_map[i].soc;
							break;
						}
					}
					

					// SOC不相等开启追踪 
					if(actual_soc != disp_soc)
					{
						// 如果显示偏小 则降速
						if(disp_soc < actual_soc)
						{
							chg_speed = soc_get_chg_speed(actual_soc - disp_soc);
							calc_incr = calc_incr * chg_speed;	 // 加速追赶						
						}
						// 如果显示偏大 则追赶
						if(disp_soc > actual_soc)
						{
							chg_speed = soc_get_chg_speed(disp_soc - actual_soc);
							calc_incr = calc_incr / chg_speed;	 // 限制速度
						}
					}
					else
					{
						calc_incr = calc_incr * CHG_SPEED_COEFF; // 平常加速
					}
					
				}
				if(soc_calc_data.rcap_mah_disp >= param_get_data(PAR_ID_FULL_CAP) * 10)
				{
					soc_calc_data.rcap_mah_disp = param_get_data(PAR_ID_FULL_CAP) * 10;
				}
				else
				{
					soc_calc_data.rcap_mah_disp += calc_incr;
				}				
			}
			else
			{
				calc_incr = calc_incr * CHG_SPEED_COEFF;
				soc_calc_data.rcap_mah_disp += calc_incr;
				if(soc_calc_data.rcap_mah_disp >= param_get_data(PAR_ID_FULL_CAP) * 10)
				{
					soc_calc_data.rcap_mah_disp = param_get_data(PAR_ID_FULL_CAP) * 10;
				}
			}
		}
			
        calc_soc = 10000 * soc_calc_data.rcap_mah_disp / (param_get_data(PAR_ID_FULL_CAP) * 10);
        var_set_data(VAR_ID_DISP_SOC , calc_soc);
        var_set_data(VAR_ID_DISP_RCAP , soc_calc_data.rcap_mah_disp * 0.1);
    }
    else /* 计算待机功耗 */
    {
		soc_calc_data.acc_iddq_mah += param_get_data(PAR_ID_BATT_SELF_POW) * 0.001 * time;
		if(soc_calc_data.acc_iddq_mah >= 36000.0)
		{
			calc_incr = soc_calc_data.acc_iddq_mah / 3600.0;
			soc_calc_data.acc_dsg_mah += calc_incr;
			if(soc_calc_data.rcap_mah_disp > calc_incr)
			{
				soc_calc_data.rcap_mah_disp -= calc_incr; 
			}
			else
				soc_calc_data.rcap_mah_disp = 0;
			
			calc_soc = 10000 * soc_calc_data.rcap_mah_disp / (param_get_data(PAR_ID_FULL_CAP) * 10);
			if(calc_soc > DISP_100_MIN)
				calc_soc = 10000;
			var_set_data(VAR_ID_DISP_SOC , calc_soc);
			var_set_data(VAR_ID_DISP_RCAP , soc_calc_data.rcap_mah_disp * 0.1);
			
			soc_calc_data.acc_iddq_mah = 0;
		}
    }
}

/**
 * @brief SOC算法处理
 */	
void soc_calc(uint16_t run_cycle)
{
	uint32_t acc_ah = 0;
    int16_t soc_diff = 0;
    uint32_t full_cap = 0;
    float calc_soh = 0;
	double now_curr = 0;
    uint8_t calc_soc = 0;
    uint32_t cycle = 0;
	uint16_t soc_cfg = 0;
    /* 计算累计AH数 */
	acc_ah = soc_calc_data.acc_chg_mah * 0.01;
	var_set_data(VAR_ID_ACC_CHG_AH_H , (acc_ah >> 16));
	var_set_data(VAR_ID_ACC_CHG_AH_L , (acc_ah  & 0xffff));
	
	acc_ah = soc_calc_data.acc_dsg_mah * 0.01;
	var_set_data(VAR_ID_ACC_DSG_AH_H , (acc_ah >> 16));
	var_set_data(VAR_ID_ACC_DSG_AH_L , (acc_ah  & 0xffff));
	
    cycle = soc_calc_data.acc_dsg_mah / (param_get_data(PAR_ID_STD_CAP) * param_get_data(PAR_ID_CYCLE_COEFF) * 0.1);
    if(cycle != param_get_data(PAR_ID_CYCLE))
	{
		if(cycle >= SOH_CYCLE_MAX)
			cycle = SOH_CYCLE_MAX;
		param_set_data(PAR_ID_CYCLE , cycle);
	}
    
	var_set_data(VAR_ID_CON_CHG_AH , soc_calc_data.acc_con_chg_mah * 0.1);
	var_set_data(VAR_ID_CON_DSG_AH , soc_calc_data.acc_con_dsg_mah * 0.1);
    
	soc_diff = var_get_data(VAR_ID_DISP_SOC) - param_get_data(PAR_ID_RSOC);
	if((soc_diff < -500) || (soc_diff > 500)) // 5% 存储一次
	{
		param_set_data(PAR_ID_RSOC , var_get_data(VAR_ID_DISP_SOC));
        param_set_data(PAR_ID_REAL_CAP , var_get_data(VAR_ID_DISP_RCAP));
		param_set_data(PAR_ID_ACC_CHG_AH_H , var_get_data(VAR_ID_ACC_CHG_AH_H));
		param_set_data(PAR_ID_ACC_CHG_AH_L , var_get_data(VAR_ID_ACC_CHG_AH_L));
		param_set_data(PAR_ID_ACC_DSG_AH_H , var_get_data(VAR_ID_ACC_DSG_AH_H));
		param_set_data(PAR_ID_ACC_DSG_AH_L , var_get_data(VAR_ID_ACC_DSG_AH_L));
	}
    
    /* 满充校准 */
    if(0 == diag_get_alarm(ALM_ID_FCHG)||0 == diag_get_prp(PRP_ID_FULL_CHG ))
    {
        if((var_get_data(VAR_ID_BATT_VOLT) > param_get_data(PAR_ID_FULL_CHG_VOLT))
			&& ((var_get_data(VAR_ID_BUS_CURR) - 30000) * 10 < param_get_data(PAR_ID_FULL_CHG_CURR))
			&& (var_get_data(VAR_ID_CURR_DR) != CURR_DR_DSG))
		{
			full_chg_dly += run_cycle;
			if(full_chg_dly >= FULL_CHG_DLY)
			{
				diag_set_alarm(ALM_ID_FCHG , 1);
				diag_set_prp(PRP_ID_FULL_CHG , 1);
				soc_calc_set_soc(10000);
				full_chg_dly = 0;
			}
		}
		else
		{
			full_chg_dly = 0;
		}
    }
    else
    {
		full_chg_dly = 0;  //要及时清零
		/* 电流小于3A 或 SOC低于98%,解除满充校准 或 最高单体电压低于单体过压告警电压-100mV*/
		now_curr = var_get_data(VAR_ID_BUS_CURR) * VAR_CURR_FACTOR - VAR_CURR_OFFSET;
        if(var_get_data(VAR_ID_DISP_SOC) * 0.01 < FCHG_RECV_SOC)
		{
            diag_set_prp(PRP_ID_FULL_CHG , 0);
			diag_set_alarm(ALM_ID_FCHG , 0);
			full_chg_dly = 0;
		}
    }

	param_get_4bits_val(PAR_ID_SOC_CFG ,FUNC_SOC_STD , &soc_cfg); 
	// 是否启动容量自学习
	if(PARAM_SOC_CFG_EN != soc_cfg)
	{
		/* 如果发生满充校准 循环次数大于20次后不自动学习 */
		if((param_get_data(PAR_ID_CYCLE) < SOH_CYCLE_BASE) && (diag_get_alarm(ALM_ID_CUV) || diag_get_prp(PRP_ID_CUV) || diag_get_prp(PRP_ID_BUV) || diag_get_alarm(ALM_ID_BUV))
			&&(var_get_data(VAR_ID_CURR_DR) != CURR_DR_DSG))
		{
			/* 自学习 */
			if(FCC_START == fcc_state)
			{
				fcc_state = FCC_CMP;
				full_cap = soc_calc_data.acc_con_dsg_mah * 0.1;
				calc_soh = 100.0 * full_cap / param_get_data(PAR_ID_STD_CAP);
				if(full_cap > 65000)
					full_cap = 65000;
			    if(PARAM_SOH_IS_VAILD(calc_soh) && (var_get_data(VAR_ID_AGV_TEMP) > 600)) 
				{
					if(calc_soh >= 110)
						calc_soh = 100;
					param_set_data(PAR_ID_SOH , calc_soh);
					param_set_data(PAR_ID_FULL_CAP , full_cap);
					soc_calc_set_soc(0);
				}
				else
				{
					fcc_state = FCC_CAP_ERR;
				}
			}
		}
		
		if(diag_get_alarm(ALM_ID_BOV) || diag_get_prp(PRP_ID_BOV)
			||diag_get_alarm(ALM_ID_COV) || diag_get_prp(PRP_ID_COV) 
			|| diag_get_alarm(ALM_ID_FCHG))
		{
			if(FCC_START != fcc_state)
			{
				soc_calc_data.acc_con_dsg_mah = 0;
				fcc_state = FCC_START;
			}
		}
		else
		{
			if(var_get_data(VAR_ID_CURR_DR) == CURR_DR_CHG)
			{
				fcc_state = FCC_IDLE;  
			}
		}
	}
	else
	{
		fcc_state = FCC_IDLE;  
	}
	// 检测SOC末端追踪是否启动
	param_get_4bits_val(PAR_ID_SOC_CFG ,FUNC_SOC_TRACE , &soc_cfg); 
	if(PARAM_SOC_CFG_EN != soc_cfg)
	{
		/* 计算SOC追踪 */
		if(var_get_data(VAR_ID_CURR_DR) == CURR_DR_DSG)
		{
			if((diag_get_prp(PRP_ID_COL) == 0) && (var_get_data(VAR_ID_MIN_CELL_VOLT) > 2000))
			{
				now_curr = var_get_data(VAR_ID_BUS_CURR) * VAR_CURR_FACTOR - VAR_CURR_OFFSET;
				calc_soc = soc_find_actual(now_curr , var_get_data(VAR_ID_MIN_CELL_VOLT));
				if(calc_soc != 0xff)
				{
					soc_calc_data.now_real_soc = calc_soc;  //真实SOC，未被用
					var_set_data(VAR_ID_RSOC , calc_soc);  //设置真实SOC，未被用
					soc_calc_data.rcap_mah_now = calc_soc * param_get_data(PAR_ID_FULL_CAP) * 0.1;  //真实容量
					if(SOC_TRACE_IDLE == soc_trace_state)
					{
						soc_trace_state = SOC_TRACE_RUN;
					}
				}
	            else
				{
					soc_trace_state = SOC_TRACE_IDLE;	
				}
			}
		}				
		else
		{
			soc_trace_state = SOC_TRACE_IDLE;
		}
	}
	else
	{
		soc_trace_state = SOC_TRACE_IDLE;
	}
	
	soc_ask_full_chg(run_cycle);
}
    
/**
 * @brief  设置当前SOC值
* @param  soc_val : 0.01%
 */
void soc_calc_set_soc(uint16_t soc_val)
{
    soc_calc_data.rcap_mah_disp = (uint32_t)soc_val * param_get_data(PAR_ID_FULL_CAP) * 0.001;
    soc_calc_data.rcap_mah_now = soc_calc_data.rcap_mah_disp;
    var_set_data(VAR_ID_DISP_SOC , soc_val);
	var_set_data(VAR_ID_DISP_RCAP , soc_calc_data.rcap_mah_disp * 0.1);
    param_set_data(PAR_ID_RSOC , soc_val);
	param_set_data(PAR_ID_REAL_CAP , soc_calc_data.rcap_mah_disp * 0.1);
}

void soc_set_acc_chg(uint32_t set_val)
{
	var_set_data(VAR_ID_ACC_CHG_AH_H , (set_val >> 16));
	var_set_data(VAR_ID_ACC_CHG_AH_L , set_val & 0xffff);
	soc_calc_data.acc_chg_mah = set_val * 10;
	param_set_data(PAR_ID_ACC_CHG_AH_H , (set_val >> 16));
	param_set_data(PAR_ID_ACC_CHG_AH_L , set_val & 0xffff);
}

void soc_set_acc_dsg(uint32_t set_val)
{
    uint16_t cycle = 0;
	var_set_data(VAR_ID_ACC_DSG_AH_H , (set_val >> 16));
	var_set_data(VAR_ID_ACC_DSG_AH_L , set_val & 0xffff);
	soc_calc_data.acc_dsg_mah = set_val * 100;
	param_set_data(PAR_ID_ACC_DSG_AH_H , (set_val >> 16));
	param_set_data(PAR_ID_ACC_DSG_AH_L , set_val & 0xffff);
    
    cycle = soc_calc_data.acc_dsg_mah * 0.1 / (param_get_data(PAR_ID_STD_CAP) * param_get_data(PAR_ID_CYCLE_COEFF) * 0.01);
	if (cycle > 6000)
		cycle = 6000;
    param_set_data(PAR_ID_CYCLE , cycle);
}

void soc_set_cycle(uint16_t set_val)
{
    uint32_t acc_dsg = 0;
    
    soc_calc_data.acc_dsg_mah = (param_get_data(PAR_ID_STD_CAP) * param_get_data(PAR_ID_CYCLE_COEFF) * 0.1) * set_val;
    acc_dsg = soc_calc_data.acc_dsg_mah * 0.01;
    param_set_data(PAR_ID_ACC_DSG_AH_H , (acc_dsg >> 16));
	param_set_data(PAR_ID_ACC_DSG_AH_L , acc_dsg & 0xffff);
}

void soc_ask_full_chg(uint16_t cycle)
{
	uint16_t now_soc = 0;
	now_soc = var_get_data(VAR_ID_DISP_SOC) * 0.01;
	if(now_soc >= 97)
	{
		no_full_chg_cnt += cycle;
		if(no_full_chg_cnt >= NO_FULL_CHG_TIME)
		{
			var_set_data(VAR_ID_ASK_FULL_CHG , 1);
			no_full_chg_cnt = NO_FULL_CHG_TIME;
		}
		else
		{
			var_set_data(VAR_ID_ASK_FULL_CHG , 0);
		}
	}
	else
	{
		var_set_data(VAR_ID_ASK_FULL_CHG , 0);
		no_full_chg_cnt = 0;
	}
}
/** ------------------------------------------------------------------------- *
  *                              Local function
 ** ------------------------------------------------------------------------- */
static float soc_get_dsg_speed(float curr)
{
	float std_cap = 0;
	float speed = 0;
	std_cap = param_get_data(PAR_ID_STD_CAP) * 0.01;
	if(curr < DSG_SPEED_CURR * std_cap)
		speed = DSG_SPEED_COEFF1;
	else if(curr < DSG_SPEED_CURR2 * std_cap)
		speed = DSG_SPEED_COEFF2;
	else 
		speed = DSG_SPEED_COEFF3;
	
	return speed;
} 

static uint16_t soc_find_actual(float bus_curr , uint16_t volt)
{
	float std_cap = param_get_data(PAR_ID_STD_CAP) * 0.01;
	uint16_t pos_x = 0;
	uint16_t pos_y = 0;
	float f_curr = 0;
	float now_curr = 0;
	float coeff = 0;
    uint16_t i = 0;
    uint16_t j = 0;
    
	if(bus_curr > 0)
		return 0xff;

	now_curr = -bus_curr;

	if(now_curr >= soc_curr_table[SOC_CURR_STEP - 1] * std_cap)
	{
		pos_x = SOC_CURR_STEP - 1;
	}
	else
	{
        for( i = 0;i < SOC_CURR_STEP; ++ i)
        {
            f_curr = std_cap * soc_curr_table[i];
            if(now_curr < f_curr)
            {
                pos_x = i;
                break;
            }
        }
	}
    
	if((0 == pos_x) || (pos_x == (SOC_CURR_STEP - 1)))
	{
        if(volt > soc_volt_table[pos_x][0])
            return 0xff;
        
		for(j = 1;j < SOC_VOLT_STEP; ++ j)
		{
			if(volt > soc_volt_table[pos_x][j])
			{
				pos_y = j - 1;
				break;
			}
		}
	}
	else
	{
		coeff = (double)(now_curr/std_cap - soc_curr_table[pos_x - 1]) / (soc_curr_table[pos_x] - soc_curr_table[pos_x - 1]);
        soc_volt_calc_buf[0] = soc_volt_table[pos_x-1][0] + (soc_volt_table[pos_x][0] - soc_volt_table[pos_x - 1][0]) * coeff;
        
        if(volt > soc_volt_calc_buf[0])
            return 0xff;
        
		for(j = 1;j < SOC_VOLT_STEP; ++ j)
		{
			soc_volt_calc_buf[j] = soc_volt_table[pos_x-1][j] + (soc_volt_table[pos_x][j] - soc_volt_table[pos_x - 1][j]) * coeff;
			if(volt > soc_volt_calc_buf[j])
			{
				pos_y = j - 1;
				break;
			}
		}
	}

	return (SOC_VOLT_STEP - 1 - pos_y);
}

static float soc_get_chg_speed(uint16_t soc_diff)
{
	if(soc_diff > 20)
		return 4.0;
	else if(soc_diff > 10)
		return 3.0;
	else 
		return 2.0;
}
/********** Copyright (c) 2023 JBD-Energy Storage Technology Co. LTD *********/
